//
// Created by Zemon on 2024/10/31.
//

#include "CAN_receive.h"
#include "main.h"


extern CAN_HandleTypeDef hcan1;//建立与两个M3508的通信
extern CAN_HandleTypeDef hcan2;
//motor data read
#define get_motor_measure(ptr, data)                                    \
    {                                                                   \
        (ptr)->last_angle = (ptr)->angle;                               \
        (ptr)->angle = (uint16_t)((data)[0] << 8 | (data)[1]);          \
        (ptr)->speed_rpm = (uint16_t)((data)[2] << 8 | (data)[3]);      \
        (ptr)->given_current = (uint16_t)((data)[4] << 8 | (data)[5]);  \
        (ptr)->temperate = (data)[6];                                   \
        if((ptr)->angle - (ptr)->last_angle > 4096)                     \
            (ptr)->round_cnt --;                                        \
        else if((ptr)->angle - (ptr)->last_angle < -4096)               \
            (ptr)->round_cnt++;                                         \
        if((ptr)->round_cnt>50||(ptr)->round_cnt<-50)                   \
            (ptr)->round_cnt = 1;                                       \
        (ptr)->total_angle = (ptr)->round_cnt * 8192 +(ptr)->angle - (ptr)->offset_angle;  \
    }                                                                   \

motor_measure_t motor_chassis[5];

static uint32_t id;
static CAN_TxHeaderTypeDef chassis_tx_message;
static uint8_t             chassis_can_send_data[8];
static HAL_StatusTypeDef  status;
static int16_t k = 0;

void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) {
    CAN_RxHeaderTypeDef rx_header;
    uint8_t rx_data[8];

    HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, rx_data);

    if(hcan == &hcan1){
        switch(rx_header.StdId)
        {
            case CAN_3508_L_ID:
            case CAN_3508_R_ID:
            case CAN_2006_H:
            case CAN_2006_store:

            {
                static uint8_t i = 0;
                i = rx_header.StdId - CAN_3508_L_ID;
                get_motor_measure(&motor_chassis[i],rx_data);
                break;
            }
            default:{
                break;
            }
        }
    }
    else if(hcan == &hcan2){
        switch (rx_header.StdId)
        {
            case CAN_6020:
            {

                get_motor_measure(&motor_chassis[4],rx_data);
                break;
            }
            default:{
                break;
            }
        }
    }

}

void CAN_cmd_chassis(int16_t motor1, int16_t motor2)
{
    k=1;
    uint32_t send_mail_box;
    chassis_tx_message.StdId = CAN_CHASSIS_ALL_ID;
    chassis_tx_message.IDE = CAN_ID_STD;
    chassis_tx_message.RTR = CAN_RTR_DATA;
    chassis_tx_message.DLC = 0x08;
    chassis_can_send_data[0] = motor1 >> 8;
    chassis_can_send_data[1] = motor1;
    chassis_can_send_data[2] = motor2 >> 8;
    chassis_can_send_data[3] = motor2;
    chassis_can_send_data[4] = 0 >> 8;
    chassis_can_send_data[5] = 0;
    chassis_can_send_data[6] = 0 >> 8;
    chassis_can_send_data[7] = 0;

    HAL_CAN_AddTxMessage(&hcan1, &chassis_tx_message, chassis_can_send_data, &send_mail_box);
}

void CAN_cmd_lift(int16_t motor3)
{
    uint32_t send_mail_box;
    chassis_tx_message.StdId = CAN_CHASSIS_ALL_ID;
    chassis_tx_message.IDE = CAN_ID_STD;
    chassis_tx_message.RTR = CAN_RTR_DATA;
    chassis_tx_message.DLC = 0x08;
    chassis_can_send_data[0] = 0 >> 8;
    chassis_can_send_data[1] = 0;
    chassis_can_send_data[2] = 0 >> 8;
    chassis_can_send_data[3] = 0;
    chassis_can_send_data[4] = 0 >> 8;
    chassis_can_send_data[5] = 0;
    chassis_can_send_data[6] = motor3 >> 8;
    chassis_can_send_data[7] = motor3;

    HAL_CAN_AddTxMessage(&hcan1, &chassis_tx_message, chassis_can_send_data, &send_mail_box);
}

/*void CAN_cmd_store(int16_t motor4)
{
    uint32_t send_mail_box;
    chassis_tx_message.StdId = CAN_CHASSIS_ALL_ID;
    chassis_tx_message.IDE = CAN_ID_STD;
    chassis_tx_message.RTR = CAN_RTR_DATA;
    chassis_tx_message.DLC = 0x08;
    chassis_can_send_data[0] = 0 >> 8;
    chassis_can_send_data[1] = 0;
    chassis_can_send_data[2] = 0 >> 8;
    chassis_can_send_data[3] = 0;
    chassis_can_send_data[4] = 0 >> 8;
    chassis_can_send_data[5] = 0;
    chassis_can_send_data[6] = motor4 >> 8;
    chassis_can_send_data[7] = motor4;

    HAL_CAN_AddTxMessage(&hcan1, &chassis_tx_message, chassis_can_send_data, &send_mail_box);
}
*/
void CAN_cmd_pitch(int16_t motor)
{

    uint32_t send_mail_box;
    chassis_tx_message.StdId = CAN_GIMBAL;
    chassis_tx_message.IDE = CAN_ID_STD;
    chassis_tx_message.RTR = CAN_RTR_DATA;
    chassis_tx_message.DLC = 0x08;
    chassis_can_send_data[0] = motor >> 8;
    chassis_can_send_data[1] = motor;
    chassis_can_send_data[2] = motor >> 8;
    chassis_can_send_data[3] = motor;
    chassis_can_send_data[4] = motor >> 8;
    chassis_can_send_data[5] = motor;
    chassis_can_send_data[6] = motor >> 8;
    chassis_can_send_data[7] = motor;

    HAL_CAN_AddTxMessage(&hcan2, &chassis_tx_message, chassis_can_send_data, &send_mail_box);
}